package clubhouse.motioncommand;

import clubhouse.base.Base;

/**
 * @author Anis Darwich & Lionel Croix - Simulation and experimentation on Robot in WSN - WnSN Lab - SJTU
 */
public class CalculInfo {
	/**Current value of distance info for the next move*/
	private int pulse_dist;
	/**Current value of rotation info for the next move*/
	private int pulse_rot;
	/**Current value of rotation direction for the next move, 1 if trigonometric direction, 0 otherwise.*/
	private int trigo;
	
	/**The coefficient linking pulses and unit, used for the moving forward information.**/
	public static final double delta = 50;	//pulses per unit
	/**The coefficient linking pulses and rad, used for the rotating information.**/
	public static final double omega = 100/Math.PI;	//pulses per rad
	
	/**
	 * Return the calculated distance info for the next move.
	 * @return the number of pulses to move forward.
	 */
	public Integer getPulse_dist() {
		return pulse_dist;
	}

	/**
	 * Return the calculated rotation info for the next move.
	 * @return the number of pulses to turn.
	 */
	public Integer getPulse_rot() {
		return pulse_rot;
	}

	/**
	 * Return the calculated rotation direction info for the next move.
	 * @return the turn direction, 1 if trigonometric direction, 0 otherwise.
	 */
	public Integer getTrigo() {
		return trigo;
	}
	
	/**
	 * Calculate the information (distance and rotation pulses) corresponding to the next step given the current position and the current destination.
	 * @param x_robot the current robot X coordinate.
	 * @param y_robot the current robot Y coordinate.
	 * @param r_robot the current robot direction.
	 * @param coord_dest the next destination coordinates to convert in pulses.
	 */
	public void calculate_move_info(double x_robot, double y_robot, double r_robot, double[] coord_dest){
		double rotation,distance;
		double offset = 0,slope=0;
		
		//Pulse Rotation
		slope = (coord_dest[1]-y_robot)/(coord_dest[0]-x_robot);

		if (coord_dest[0] - x_robot < 0){
			offset = -Math.PI;
		}
		rotation = Math.atan(slope) - r_robot + offset;
		if (rotation <= -Math.PI) rotation = rotation + 2*Math.PI;
		if (rotation > Math.PI) rotation = rotation - 2*Math.PI;
		if (Base.DEBUG) System.out.println("(calculate_move_info) slope = "+slope+", atan = "+Math.atan(slope)+" rd, r_robot = "+r_robot+", offset = "+offset);
		this.pulse_rot = (int) Math.round(Math.abs(rotation*CalculInfo.omega));
		if (Base.DEBUG) System.out.println("(calculate_move_info) rotation = "+rotation+" rd, pulses = "+this.pulse_rot);
			
		//Trigo rotation
		if (rotation >= 0){
			this.trigo = 1;
		} else {
			this.trigo = 0;
		}
		if (Base.DEBUG) System.out.println("(calculate_move_info) trigo = "+this.trigo);
		
		//Pulse Distance
		distance = Base.euclideanDist(x_robot, y_robot, coord_dest[0], coord_dest[1]);
		if (distance > 1.5){
			distance = 1;
		}
		this.pulse_dist = (int) Math.round(distance * CalculInfo.delta);
		if (Base.DEBUG) System.out.println("(calculate_move_info) distance = "+distance+" unit(s), pulses = "+this.pulse_dist);
	}
}
